#include "State_JumpR.h"
#include "Dog.h"


#define preANGLE2_F RAD(30)//在缩腿站立状态下需继续旋转的角度
#define preANGLE2_B RAD(30)
#define POINTANGLE1 RAD(100)

#define KP0 8.0f
#define KD0 1.3f
#define KP1 8.0f
#define KD1 1.3f
#define KP2 8.0f
#define KD2 1.3f
#define KP3 8.0f
#define KD3 1.3f

void JumpR_Enter(void)
{
    UART_Print("JumpR enter\n");
}

void JumpR_Run(void)
{
    /*曲腿准备*/
    /*HT_SetPosition(dog.legs[0].motorF, ANGLE_F+preANGLE2_F, KP0, KD0, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F+preANGLE2_F, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F+preANGLE2_F, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F+preANGLE2_F, KP3, KD3, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B-preANGLE2_B, KP0, KD0, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B-preANGLE2_B, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B-preANGLE2_B, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B-preANGLE2_B, KP3, KD3, 0);*/

    /*旋钮控制*/
    HT_SetPosition(dog.legs[0].motorF, ANGLE_F + global.jumpAngleR, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorF, -ANGLE_F+global.jumpAngleL, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorF, -ANGLE_F+global.jumpAngleL, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorF, ANGLE_F + global.jumpAngleR, KP2, KD2, 0);

    HT_SetPosition(dog.legs[0].motorB, -ANGLE_B - global.jumpAngleR, KP1, KD1, 0);
    HT_SetPosition(dog.legs[1].motorB, ANGLE_B-global.jumpAngleL, KP1, KD1, 0);
    HT_SetPosition(dog.legs[2].motorB, ANGLE_B-global.jumpAngleL, KP2, KD2, 0);
    HT_SetPosition(dog.legs[3].motorB, -ANGLE_B - global.jumpAngleR, KP2, KD2, 0);
    
}

void JumpR_Exit(void)
{
    UART_Print("JumpR exit\n");
}
